#include "KochanekBartelsCurve.h"
#include <cassert>
#include <math.h>

KochanekBartelsCurve::KochanekBartelsCurve(const std::vector<Snapshot>& snapshots, float tension, float bias) : m_nodes(snapshots.size())
{
	//if(snapshots.size() == 0)
	//{
	//	return DirectX::XMVectorSet(0.f, 0.f, 0.f, 1.f);
	//}
	m_nodes.clear();
	std::vector<Snapshot>::const_iterator snapShotIterator;
	for(snapShotIterator = snapshots.begin(); snapShotIterator != snapshots.end(); ++snapShotIterator)
	{
		CurveNode node;
		DirectX::XMFLOAT4 center(0.f, 0.f, 0.f, 1.f);
		DirectX::XMVECTOR location = DirectX::XMLoadFloat4(&center);
		DirectX::XMMATRIX translationMatrix = snapShotIterator->translation;
		location = DirectX::XMVector4Transform(location, translationMatrix);
		DirectX::XMStoreFloat4(&node.location, location);
		m_nodes.push_back(node);
	}
	//all locations are set -> calculate tangents
	for(int i = 0; i < static_cast<int>(m_nodes.size()); ++i)
	{
		using namespace DirectX;
		DirectX::XMVECTOR locationPrecedent, locationActual, locationSubsequent;
		locationPrecedent = XMLoadFloat4(&m_nodes[(i - 1 + m_nodes.size()) % m_nodes.size()].location);
		locationActual = XMLoadFloat4(&m_nodes[i].location);
		locationSubsequent = XMLoadFloat4(&m_nodes[(i + 1) % m_nodes.size()].location);
		DirectX::XMVECTOR tangentActual;
		float mod1, mod2;
		mod1 = ((1 - tension) * (1 + bias) / 2);
		mod2 = ((1 - tension) * (1 - bias) / 2);
		tangentActual = mod1 * (locationActual - locationPrecedent) + mod2 * (locationSubsequent - locationActual);
		XMStoreFloat4(&m_nodes[i].tangent, tangentActual);
	}
}

KochanekBartelsCurve::~KochanekBartelsCurve(void)
{
	m_nodes.clear();
}

DirectX::XMVECTOR KochanekBartelsCurve::GetLocation( const double time ) const
{
	using namespace DirectX;
	if(m_nodes.size() == 0)
	{
		return DirectX::XMVectorSet(0.f, 0.f, 0.f, 1.f);
	}
	XMVECTOR p1, p2, t1, t2;
	assert(time >= 0);
	int n1 = static_cast<int>(floor(time)) % m_nodes.size();
	int n2 = (n1 + 1) % m_nodes.size();
	p1 = XMLoadFloat4(&m_nodes[n1].location);
	p2 = XMLoadFloat4(&m_nodes[n2].location);
	t1 = XMLoadFloat4(&m_nodes[n1].tangent);
	t2 = XMLoadFloat4(&m_nodes[n2].tangent);
	double t = fmod(time, 1.);
	DirectX::XMVECTOR p = static_cast<float>((2*pow(t, 3) - 3*pow(t, 2) + 1)) * p1
		+ static_cast<float>((pow(t, 3) - 2*pow(t, 2) + t)) * t1
		+ static_cast<float>((pow(t, 3) - pow(t, 2))) * t2
		+ static_cast<float>((-2*pow(t, 3) + 3*pow(t,2))) * p2;
	return p;
}